#include "gcode.h"
#include <arm_math.h>
uint8_t cicr_ex = 0;

typedef struct
{
	float start_x;
	float start_y;
	float stop_x;
	float stop_y;
}cicur_ex_param_t;
cicur_ex_param_t cicur_ex_param[5];

typedef struct
{
    uint8_t status_code; //状态
    uint8_t inches_mode;             /* 0 = millimeter mode, 1 = inches mode {G20, G21} */ //英寸和毫米模式
    uint8_t absolute_mode;           /* 0 = relative motion, 1 = absolute motion {G90, G91} */// 绝对运动和相对运动模式
    uint8_t spindle_on, extruder_on;  //主轴和挤出机
    double feed_rate, seek_rate;     /* Millimeters/second */  //毫米/秒    定位速度和插补速度
    double extruder_length, extruder_k; //挤出机长度和比例系数K
    double position[3];              /* Where the interpreter considers the tool to be at this point in the code */ //当前位置坐标
    short s_value;           /* RPM/100 or temperature of the current extruder */ //挤出机目前转速
    uint8_t next_action;  /* The action that will be taken by the parsed line */ //下一个将要采取的解析行动
} parser_state_t;
parser_state_t gc; //定义了一个结构体


double strtod_M(const char* str, char** endptr) //读取字符串数字
{
    double number = 0.0;
    double div = 0.0;
    bool negative = false;
    bool plus = false;
    bool skip = true;
    char c;

    while ((c = *str) != 0)
    {
        if (c == '+')
        {
            if (skip && !plus)
            {
                plus = true;
                skip = false;
            }
            else
                break;
        }
        else if (skip && !negative && c == '-')
        {
            if (skip && !negative)
            {
                negative = true;
                skip = false;
            }
            else
                break;
        }
        else if (c == '.')
        {
            if (div == 0.0)
                div = 1.0;
            else
                break;
        }
        else if (c >= '0' && c <= '9')
        {
            skip = false;

            if (div == 0.0)
                number = number * 10.0 + (double)(c - '0');
            else
            {
                div *= 10.0;
                number += ((double)(c - '0') / div);
            }
        }
        else if (!skip)
        {
            break;
        }

        str++;
    }

    if (negative) number = -number;

    if (endptr != NULL) *endptr = (char*)str;

    return number;
}


static int read_double(char* line, int* char_counter, double* double_ptr) //读取双精度值
{
    char* start = line + *char_counter;
    char* end;

    *double_ptr = strtod_M(start, &end);

    if (end == start)
    {
        gc.status_code = GCSTATUS_BAD_NUMBER_FORMAT;  //糟糕的数据模式
        return false;
    }

    *char_counter = (int)(end - line);
    return true;
}

static int next_statement(char* letter, double* double_ptr, char* line, int* char_counter)  //过滤G代码字符串
{
    while (line[*char_counter] == ' ') (*char_counter)++;

    if (line[*char_counter] == 0 || line[*char_counter] == ';' ||
            line[*char_counter] == '\n' || line[*char_counter] == '\r') return false;

    *letter = line[*char_counter];

    if ((*letter < 'A') || (*letter > 'Z'))
    {
        gc.status_code = GCSTATUS_EXPECTED_COMMAND_LETTER;  //非预期的命令值字母
        return false;
    }

    (*char_counter)++;
    return read_double(line, char_counter, double_ptr); //读值
}


uint8_t gc_execute_line(char* line)
{
    double feed_rate;
    double extrudeLength;
    int char_counter = 0;
    char letter;
    double value;
    double offset[3] = {0};
    double radius = 0;
    int pause_value = 0;
    uint8_t radius_mode = false;
	char start_quadrant = 0;
	char stop_quadrant = 0;
	
	int X_postion = 0;
	int Y_postion = 0;
	int Z_postion = 0;
    gc.status_code = GCSTATUS_OK;  //状态OK

    while (next_statement(&letter, &value, line, &char_counter))
    {
        int int_value = (int)value;

        switch (letter)
        {
            case 'N':
                break;//遇到N直接返回

            case 'G':
                switch (int_value)
                {
                    case 0:
                        gc.next_action = NEXT_ACTION_SEEK_G0;
                        break; //快速移动

                    case 1:
                        gc.next_action = NEXT_ACTION_LINEAR_G1;
                        break;  //直线插补

                    case 2:
                        gc.next_action = NEXT_ACTION_CW_ARC;
                        break;  //顺圆插补

                    case 3:
                        gc.next_action = NEXT_ACTION_CCW_ARC;
                        break; //逆时针圆弧插补

                    case 4:
                        gc.next_action = NEXT_ACTION_DWELL_G4;
                        break;  //暂停

                    case 30:
                    case 28:
                        gc.next_action = NEXT_ACTION_GO_HOME_G28;
                        break;  //返回原点

                    case 90:
                        gc.absolute_mode = true;
                        break;

                    case 91:
                        gc.absolute_mode = false;
                        break; //绝对模式还是相对模式

                    case 92:
                        gc.next_action = NEXT_ACTION_RESET_XYZ_G92;
                        break;  //XYZ坐标复位

                    case 64:
                    case 40:
                    case 17:	// G17  选择加工平面
                    case 94:	// Feedrate per minute //进给速率模式
                    case 98:	// Feedrate per minute (group type A) //每分钟进给速度  另一个模组
                    case 97:	// Constant spindle speed M T Takes an S addr ess integer, which is interpreted as rev/min (rpm).

                    //The default speed mode per system parameter if no mode is programmed. 主轴转速
                    case 49:	// Tool length offset compensation cancel  //道具长度补偿
                    case 80:	// Cancel canned cycle  固定循环取消
                        break;

                    default:
                        printf("不支持的G指令 error:%d\r\n", gc.status_code);
                        break;
                }

                break;

            case 'M':
                switch (int_value)
                {
                    case 112: // Emergency Stop   急停
                    case 0:
                    case 1:
                    case 2:
                    case 30:
                    case 60:  //取消
                        gc.next_action = NEXT_ACTION_STOP;
                        break;

                    case 3:
                        gc.spindle_on = 1;
                        break;//主轴开启

                    //	case 4: gc.spindle_direction = -1; break;   //方向
                    case 5:
                        gc.spindle_on = 0;
                        break;//主轴关闭

                    default:
                        printf("不支持的M指令 error : % d\r\n", gc.status_code);
                        break;
                }

                break;

            default:
                printf("不支持的指令 error:%d char:%c\r\n", gc.status_code, letter);
                break;
        }
    }

    if (gc.status_code)
        return gc.status_code;

    char_counter = 0;

    while (next_statement(&letter, &value, line, &char_counter))
    {
        double unit_millimeters_value = value;

        switch (letter)
        {
            case 'F':
                if (gc.next_action == NEXT_ACTION_SEEK_G0)  //如果下一条指令为匡苏定位 则更新快速定位的速度
                    gc.seek_rate = unit_millimeters_value;               //快速定位速度
                else
                    gc.feed_rate = unit_millimeters_value; // millimeters pr min  毫米每分钟

                break;

            case 'P':
                pause_value = (int)value;
                break;  // 暂停时间

            case 'S':
                gc.s_value = (short)value;
                break;  //主轴转速

            case 'X':
            case 'Y':
            case 'Z':
                if (gc.absolute_mode)     //判断坐标系模式
                    gc.position[letter - 'X'] = unit_millimeters_value;
                else
                    gc.position[letter - 'X'] += unit_millimeters_value;   //得到当前XYZ坐标

                break;

            case 'I':
            case 'J':
            case 'K':
                offset[letter - 'I'] = unit_millimeters_value;  //得到偏移量
                break;

            case 'R':
                radius = unit_millimeters_value;
                radius_mode = true;             //如果为R则为半径模式，并开始半径模式
                break;

            case 'G':
            case 'N':
            case 'M':
                break;

            default:
                printf("不支持的辅助码\r\n");
                break;
        }
    }

    if (gc.status_code)
        return gc.status_code; //判断当前解析器状态

    feed_rate = (gc.next_action == NEXT_ACTION_SEEK_G0) ? gc.seek_rate : gc.feed_rate;  //  算法解析  max=(a>b)?a:b    如果a>b,则把a的值赋给max,否则b的值赋max

    switch (gc.next_action)//执行动作
    {
        case NEXT_ACTION_DWELL_G4:   //执行暂定动作。
			CNC_DELAY(pause_value);
            printf("G4暂停，时间%d\r\n", pause_value);
            break;

        case NEXT_ACTION_RESET_XYZ_G92:
			MC_Execute_param.X_work_current = 0;
			MC_Execute_param.Y_work_current = 0;
			MC_Execute_param.Z_work_current = 0;
			MC_Execute_param.X_offset =MC_Execute_param.X_current - MC_Execute_param.X_work_current;
			MC_Execute_param.Y_offset =MC_Execute_param.Y_current - MC_Execute_param.Y_work_current;
			MC_Execute_param.Z_offset =MC_Execute_param.Z_current - MC_Execute_param.Z_work_current;			
//           printf("G92复位XYZ坐标\r\n");
            break;

        case NEXT_ACTION_STOP:
			circular_para.endpoint_pulse = 0;
			interpolation_para.endpoint_pulse =0;
			z_axis_pul = 0;
            printf("停止");
            break;

        case NEXT_ACTION_GO_HOME_G28:
			Liner_interpolationEx(-MC_Execute_param.X_current,-MC_Execute_param.Y_current,feed_rate);
			printf("快速归零G28 G0\r\n");
			break;
        case NEXT_ACTION_SEEK_G0:	
		{
			X_postion = (int)gc.position[X_AXIS]*10 + MC_Execute_param.X_offset - MC_Execute_param.X_current;
			Y_postion = (int)gc.position[Y_AXIS]*10 + MC_Execute_param.Y_offset - MC_Execute_param.Y_current;
			if(X_postion == 0 && Y_postion == 0)
			{
				break;
			}
            Liner_interpolationEx(X_postion,Y_postion,feed_rate);
			printf("快速归零G0 \r\n");
            break;
		}
        case NEXT_ACTION_LINEAR_G1: //如果为直线插补
			X_postion = (int)(gc.position[X_AXIS]*10) + MC_Execute_param.X_offset - MC_Execute_param.X_current;
			Y_postion = (int)(gc.position[Y_AXIS]*10) + MC_Execute_param.Y_offset - MC_Execute_param.Y_current;
			Z_postion = (int)(gc.position[Z_AXIS]*10) + MC_Execute_param.Z_offset - MC_Execute_param.Z_current;
			if(X_postion != 0 || Y_postion != 0)
			{
				if(!limited_flag[1] && !limited_flag[0])
				{
					Liner_interpolationEx(X_postion,Y_postion,feed_rate);	
				}
				else
				{
					if(X_postion > 0 && Y_postion > 0)
					{
						limit_en[0] = 0;
						limit_en[1] = 0;
						Liner_interpolationEx(X_postion,Y_postion,feed_rate);
					}				
				}
							
			}

			if(Z_postion != 0)
			{
				if(!limited_flag[2])
				{
					Z_axisEx(Z_postion,feed_rate);
				}
				else
				{
					if(Z_postion < 0)
					{
						limit_en[2] = 0;
						Z_axisEx(Z_postion,feed_rate);
					}
				}
			}
				
			printf("直线插补 X:%.1f Y:%.1f Z:%.1f F:%.1f\r\n", gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS], feed_rate);
            break;

        case NEXT_ACTION_CW_ARC:
        case NEXT_ACTION_CCW_ARC:		
            if (!radius_mode)
            {
				float stop_x = gc.position[X_AXIS];
				float stop_y = gc.position[Y_AXIS];
				printf("stop_x:%.1f \r\n stop_y:%.1f\r\n",stop_x,stop_y);
				float inc_start_x = -offset[X_AXIS];
				float inc_start_y = -offset[Y_AXIS];
				float center_x = MC_Execute_param.X_work_current/10.0 + offset[X_AXIS];
				printf("x offset %.1f\r\n",offset[X_AXIS]);
				printf("center_x %.1f\r\n",center_x);
				float center_y = MC_Execute_param.Y_work_current/10.0 + offset[Y_AXIS];
				printf("y offset %.1f\r\n",offset[Y_AXIS]);
				printf("center_y %.1f\r\n",center_y);
				float inc_stop_x = stop_x - center_x;
				float inc_stop_y = stop_y - center_y;
				
				if((inc_start_x * inc_stop_x + inc_start_y * inc_stop_y) < 0)
					return 10;
				printf("一个象限\r\n");
				printf("inc_start_x %0.1f\r\ninc_start_y %0.1f\r\ninc_stop_x %0.1f \r\ninc_stop_y%0.1f\r\n",
						inc_start_x,inc_start_y,inc_stop_x,inc_stop_y);
				if(gc.next_action==NEXT_ACTION_CW_ARC)
					Circular_interpolationEx((int)(inc_start_x*10),(int)(inc_start_y*10),(int)(inc_stop_x*10),(int)(inc_stop_y*10),feed_rate,CW);
				else if(gc.next_action==NEXT_ACTION_CCW_ARC)
					Circular_interpolationEx((int)(inc_start_x*10),(int)(inc_start_y*10),(int)(inc_stop_x*10),(int)(inc_stop_y*10),feed_rate,CCW);
				printf("error:");
            }
			else
			{
				Circular_interpolationEx(100,0,0,100,250,CW);
				while(circular_para.motionstatus);
				Circular_interpolationEx(0,100,-100,0,250,CW);
				while(circular_para.motionstatus);
				Circular_interpolationEx(-100,0,0,-100,250,CW);
				while(circular_para.motionstatus);
				Circular_interpolationEx(0,-100,100,0,250,CW);
				printf("半径模式\r\n");
				printf("圆弧插补 X:%.1f Y:%.1f Z:%.1f F:%.1f\r\n", gc.position[X_AXIS], gc.position[Y_AXIS], gc.position[Z_AXIS], gc.feed_rate);
				printf("圆弧插补 I:%.1f J:%.1f K:.1%.1f R:%.1f\r\n", offset[X_AXIS], offset[Y_AXIS], offset[Z_AXIS], radius);			
			}

            break;

        default:
            break;
    }

    return gc.status_code;
}


void gc_init(void) //解析器初始化
{
    memset(&gc, 0, sizeof(gc));
    gc.feed_rate = SM_DEFAULT_FEED_RATE;
    gc.seek_rate = SM_DEFAULT_SEEK_RATE;
    gc.absolute_mode = true;
    gc.extruder_k = 1;  //比例系数设为1
    // commonValues.extruder_k;
    gc.next_action = NEXT_ACTION_DEFAULT;
}

void gc_execute(char parameter)
{
	static uint8_t int_ex_count = 0;
	if(!parameter)
		return;
	if(circular_para.motionstatus == 0 || int_ex_count == 0)
	{
		Circular_interpolationEx(cicur_ex_param[int_ex_count].start_x * 10,cicur_ex_param[int_ex_count].start_y* 10,
									cicur_ex_param[int_ex_count].stop_x* 10,cicur_ex_param[int_ex_count].stop_y* 10,circular_para.speed,circular_para.dir);
		int_ex_count ++;

	}
	
}


